#include "../include/legdetector_vis/visualizer.h"

LegVisualizer::LegVisualizer():nh()
{

}

bool LegVisualizer::init()
{
    try
    {
    legSubscriber= nh.subscribe<hri_msgs::Legs>("/legs",100,
                                                 (boost::function<void(const hri_msgs::LegsConstPtr&)>)
                                                boost::bind(&LegVisualizer::legsSubscriberCB, this, _1));

    leg_marke_publisher=nh.advertise<visualization_msgs::MarkerArray>("/legs_marker",100);

    transform_listener= new tf::TransformListener(ros::Duration(1000));
    }
    catch(...)
    {
      return false;
    }
    return true;

}

int LegVisualizer::run()
{
//    ros::Rate loop_rate(100);
//    while(ros::ok())
//    {
//        ros::spinOnce();
//        loop_rate.sleep();
//    }
    ros::spin();
    return 0;

}

void LegVisualizer::legsSubscriberCB(const hri_msgs::LegsConstPtr& legs)
{
    visualization_msgs::MarkerArray marker_array;
    int id = 0;
    for (uint i = 0; i < legs->legs.size(); i++)
    {
        if(legs->legs[i].confidence.data >= 0.5)
        {
            if(legs->legs[i].isPair.data)
            {
                std::cout<<"HIGHER "<<id<<std::endl;
                marker_array.markers.push_back(createMarker(legs->legs[i].center_of_leg1,id,
                                                            legs->legs[i].isPair.data));
                id++;
                marker_array.markers.push_back(createMarker(legs->legs[i].center_of_leg2,id,
                                                            legs->legs[i].isPair.data));
            }
            else
            {
                marker_array.markers.push_back(createMarker(legs->legs[i].center_of_leg1,id,
                                                            legs->legs[i].isPair.data));

            }
            id++;
        }
    }
    leg_marke_publisher.publish(marker_array);
}

geometry_msgs::PoseStamped LegVisualizer::transform(geometry_msgs::Pose pose)
{
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.pose=pose;
    pose_stamped.pose.orientation.w = 1;
    pose_stamped.pose.orientation.x = 0;
    pose_stamped.pose.orientation.y = 0;
    pose_stamped.pose.orientation.z = 0;

    pose_stamped.header.frame_id="/laser";
    pose_stamped.header.stamp=ros::Time::now();
    geometry_msgs::PoseStamped transformed_pose;
    bool success = false;
    transformed_pose = pose_stamped;
    /*while(!success && ros::ok())
    {
        try
        {
            if(transform_listener->waitForTransform("/base_link","/laser",ros::Time(),ros::Duration(50)))
            {
                transform_listener->transformPose("/base_link",pose_stamped,transformed_pose);
                success = true;
            }
        }

        catch(tf::ExtrapolationException e)
        {
        }
    }*/
    return transformed_pose;
}

visualization_msgs::Marker LegVisualizer::createMarker(geometry_msgs::Pose leg_pose, int id,bool pair)
{
    visualization_msgs::Marker marker;
    marker.pose=transform(leg_pose).pose;
//    marker.pose.position.x/=100;
//    marker.pose.position.y/=100;
    marker.header.frame_id = "/laser";
    marker.header.stamp = ros::Time::now();
    marker.ns = "detected_legs";
    marker.id = id;
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1;
    if(!pair)
    {
    marker.color.r = 1.0f;
    marker.color.g = 0.0f;
    }
    else
    {
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
    }
    marker.color.b = 0.0f;
    marker.color.a = 1.0f;

    marker.scale.x = 0.05;
    marker.scale.y = 0.05;
    marker.scale.z = 0.05;
    marker.text = "Human_leg";
    marker.lifetime = ros::Duration();
    return marker;
}
